#include "Vector3.h"

#include <cassert>
#include <math.h>
#include <string.h>

#include "Point3.h"
#include "Utils.h"

Vector3::Vector3(const float x1 /*= 0.0f*/, 
                 const float y1 /*= 0.0f*/, 
                 const float z1 /*= 0.0f*/)
                 : x(x1)
                 , y(y1)
                 , z(z1)
{

}

Vector3::Vector3(const Point3& from, const Point3& to) 
    : x(to.x - from.x)
    , y(to.y - from.y)
    , z(to.z - from.z)
{

}

bool Vector3::operator==(const Vector3& v) const {
    return areEquals(x, v.x) && 
        areEquals(y, v.y) &&
        areEquals(z, v.z);
}

bool Vector3::operator!=(const Vector3& v) const {
    return !(*this == v);
}

Vector3& Vector3::operator+=(const Vector3& v) {
    x += v.x;
    y += v.y;
    z += v.z;

    return *this;
}

Vector3& Vector3::operator-=(const Vector3& v) {
    x -= v.x;
    y -= v.y;
    z -= v.z;

    return *this;
}

void Vector3::set(const float x1, const float y1, const float z1) {
    x = x1;
    y = y1;
    z = z1;
}

void Vector3::set(const Point3& from, const Point3& to) {
    x = to.x - from.x;
    y = to.y - from.y;
    z = to.z - from.z;
}

float dotProduct(const Vector3& v1, const Vector3& v2) {
    return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
}

void crossProduct(const Vector3& v1, const Vector3& v2, Vector3& out) {
    assert(&out != &v1);
    assert(&out != &v2);

    out.x = v1.y * v2.z - v1.z * v2.y;
    out.y = v1.z * v2.x - v1.x * v2.z;
    out.z = v1.x * v2.y - v1.y * v2.x;
}

void Vector3::crossProduct(const Vector3& v1) {
    x = y * v1.z - z * v1.y;
    y = z * v1.x - x * v1.z;
    z = x * v1.y - y * v1.x;
}

void normalize(const Vector3& v, Vector3& out) {
    const float len = v.length();

    if (len > 0.0f) {
        const float invLen = 1.0f / len;
        out.x = v.x * invLen;
        out.y = v.y * invLen;
        out.z = v.z * invLen;
    }
}

void Vector3::normalize() {
    const float len = length();

    if (len > 0.0f) {
        const float invLen = 1.0f / len;
        x *= invLen;
        y *= invLen;
        z *= invLen;
    }
}

bool Vector3::isZero() const{
    return areEquals(x, 0.0f) && 
        areEquals(y, 0.0f) &&
        areEquals(z, 0.0f);
}

float Vector3::length() const{
    return sqrt(sqrLength());
}

float Vector3::sqrLength() const{
    return x * x + y * y + z * z;
}

void Vector3::setLength(const float newLength) {
    assert(areEquals(newLength, 0.0f) || 0.0f < newLength);

    normalize();
    x *= newLength;
    y *= newLength;
    z *= newLength;
}

Point3 Vector3::operator+(const Point3& p) const {
    const float newX = p.x + x;
    const float newY = p.y + y;
    const float newZ = p.z + z;

    return Point3(newX, newY, newZ);
}

Vector3 Vector3::operator+(const Vector3& v) const {
    const float newX = x + v.x;
    const float newY = y + v.y;
    const float newZ = z + v.z;

    return Vector3(newX, newY, newZ);
}

Vector3 Vector3::operator-(const Vector3& v) const {
    const float newX = x - v.x;
    const float newY = y - v.y;
    const float newZ = z - v.z;

    return Vector3(newX, newY, newZ);
}

Vector3& Vector3::operator*=(const float s) {
    x *= s;
    y *= s;
    z *= s;

    return *this;
}

Vector3 Vector3::operator*(const float s) const {
    const float newX = x * s;
    const float newY = y * s;
    const float newZ = z * s;

    return Vector3(newX, newY, newZ);
}